/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "Quaternion.h"
#include <iostream>

Quaternion::Quaternion() {
	x = 0;
	y = 0;
	z = 0;
	w = 1;
}

Quaternion::Quaternion(double x, double y, double z, double angle) {
    double omega, s;
    double l = sqrt(x * x + y * y + z * z);

    omega = -0.5 * angle;
    s = sin(omega) / l;

    this->x = s * x;
    this->y = s * y;
    this->z = s * z;
    this->w = cos(omega);
}

void Quaternion::setRpy(float roll, float pitch, float yaw) {
	// calculate rotation quaternion from roll pitch yaw (XYZ convention)
	double factor = (2*M_PI)/(2*360);
	double sr = sin( factor*roll ); double cr = cos( factor*roll );
	double sp = sin( factor*pitch ); double cp = cos( factor*pitch );
	double sy = sin( factor*yaw ); double cy = cos( factor*yaw );
    w = cr * cp * cy + sr * sp * sy;
    x = sr * cp * cy - cr * sp * sy;
    y = cr * sp * cy + sr * cp * sy;
    z = cr * cp * sy - sr * sp * cy;
}

void Quaternion::getRpy(double *rpy) {
	double factor = 360.0/(2*M_PI);
	rpy[0] = factor*atan2(2*y*z + 2*w*x, z*z - y*y - x*x + w*w);
	rpy[1] = -factor*asin(2*x*z - 2*w*y);
	rpy[2] = factor*atan2(2*x*y + 2*w*z, x*x + w*w - z*z - y*y);
}
float Quaternion::getRoll() {
	double factor = 360.0/(2*M_PI);
	return factor*atan2(2*y*z + 2*w*x, z*z - y*y - x*x + w*w);
}
float Quaternion::getPitch() {
	double factor = 360.0/(2*M_PI);
	return -factor*asin(2*x*z - 2*w*y);
}
float Quaternion::getYaw() {
	double factor = 360.0/(2*M_PI);
	return factor*atan2(2*x*y + 2*w*z, x*x + w*w - z*z - y*y);
}
void Quaternion::getMatrix(double *mat) {
	double xx = 2*x*x;
	double yy = 2*y*y;
	double zz = 2*z*z;
  
	mat[0] = 1 - yy - zz;
	mat[1] = 2*(x*y - w*z);
	mat[2] = 2*(x*z + w*y);
	mat[3] = 2*(x*y + w*z);
	mat[4] = 1 - xx - zz;
	mat[5] = 2*(y*z - w*x);
	mat[6] = 2*(x*z - w*y);
	mat[7] = 2*(y*z + w*x);
	mat[8] = 1 - xx - yy;
}

Quaternion Quaternion::operator * (const Quaternion& q2) {
	Quaternion q;
    q.x =  x * q2.w + y * q2.z - z * q2.y + w * q2.x;
    q.y = -x * q2.z + y * q2.w + z * q2.x + w * q2.y;
    q.z =  x * q2.y - y * q2.x + z * q2.w + w * q2.z;
    q.w = -x * q2.x - y * q2.y - z * q2.z + w * q2.w;
    return q;
}

Quaternion Quaternion::operator- () {
	Quaternion q;
    q.x = x;
    q.y = y;
    q.z = z;
    q.w = -w;
    return q;
}

void Quaternion::rotate(double *v) {
	double a = w*x;
	double b = w*y;
	double c = w*z;
	double d = -x*x;
	double e = x*y;
	double f = x*z;
	double g = -y*y;
	double i = y*z;
	double j = -z*z;
	double v0 = 2*( (g + j)*v[0] + (e -  c)*v[1] + (b + f)*v[2] ) + v[0];
	double v1 = 2*( (c +  e)*v[0] + (d + j)*v[1] + (i - a)*v[2] ) + v[1];
	double v2 = 2*( (f -  b)*v[0] + (a +  i)*v[1] + (d + g)*v[2] ) + v[2];
	
	v[0] = v0;
	v[1] = v1;
	v[2] = v2;
}

Vector3D Quaternion::operator* (const Vector3D& v) {
	double a = w*x;
	double b = w*y;
	double c = w*z;
	double d = -x*x;
	double e = x*y;
	double f = x*z;
	double g = -y*y;
	double i = y*z;
	double j = -z*z;
	double v0 = 2*( (g + j)*v.x + (e -  c)*v.y + (b + f)*v.z ) + v.x;
	double v1 = 2*( (c +  e)*v.x + (d + j)*v.y + (i - a)*v.z ) + v.y;
	double v2 = 2*( (f -  b)*v.x + (a +  i)*v.y + (d + g)*v.z ) + v.z;
	
	Vector3D res;
	res.x = v0;
	res.y = v1;
	res.z = v2;
	return res;
}
